#include "PHYModeler.h"
#include "cd_wavefront.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
//#include "btCollisionObject.h"
#include <iostream>
using namespace std;

PHYModeler *PHYModeler::pModeler = new PHYModeler();
PHYModeler::PHYModeler()
{
	/*collision configuration contains default setup for memory, collision setup. 
	Advanced users can create their own configuration.*
	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();

	/*use the default collision dispatcher. 
	For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)*
	btCollisionDispatcher* dispatcher = new	btCollisionDispatcher(collisionConfiguration);

	/*btDbvtBroadphase is a good general purpose broadphase.
	You can also try out btAxis3Sweep.*
	btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase();

	/*the default constraint solver. 
	For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)*
	btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
	/**/
	btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
	btCollisionDispatcher* dispatcher = new	btCollisionDispatcher(collisionConfiguration);
		
	btVector3 maxAabb(10000,10000,10000);
	btBroadphaseInterface* broadphase = new btAxisSweep3(-maxAabb,maxAabb);//SimpleBroadphase();
	dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,GIMPACT_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc);
	
	btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
	_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,constraintSolver,collisionConfiguration);


	//_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);
	_dynamicsWorld->setGravity(btVector3(0.,-1.5,0.));

	//_CreateGround();
}
void PHYModeler::BeginSimulation()
{
	_dynamicsWorld->stepSimulation(1);
}
PHYModeler* PHYModeler::getInstance()
{
	if(!pModeler)
		pModeler = new PHYModeler();
	return pModeler;
}
PHYModeler::~PHYModeler()
{
	delete pModeler;
}
void PHYModeler::AddCollisionObject(PHYCollisionObj *ipObj)
{
	cout << "PHYModeler::AddCollisionObject" << endl;
	_CollisionObjects.push_back(ipObj);
	_CreateCollisionBody(ipObj);
}
void PHYModeler::_CreateCollisionBody(PHYCollisionObj *ipObj)
{
	cout << "PHYModeler::AddCollisionObject" << endl;
	if(!ipObj)
		return;
	PHY_Shapes shp = ipObj->getShape();
	btCollisionShape* shape = NULL;
	btConvexHullShape* convexShape = NULL;
	btShapeHull* hull = NULL;
	btConvexShape* tmpConvexShape = NULL;
	btTriangleMesh* trimesh = NULL;
	ConvexDecomposition::WavefrontObj wo;
	int count = 0;
	PHYSphere *pSph = NULL;
	PHYBox *pBox = NULL;
	PHYConvex *pConvex = NULL;
	btTransform location;
	location.setIdentity();
	btScalar mass(0.);
	switch(shp)
	{
	case PHY_Sphere:
		pSph = (PHYSphere *)ipObj;
		shape = new btSphereShape(pSph->getRadius());
		location.setOrigin(btVector3(pSph->getLocation().getX(),pSph->getLocation().getY(),pSph->getLocation().getZ()));
		mass = 1.;
		break;
	case PHY_Box:
		pBox = (PHYBox *)ipObj;
		shape = new btBoxShape(btVector3(pBox->getX(),pBox->getY(),pBox->getZ()));
		location.setOrigin(btVector3(pBox->getLocation().getX(),pBox->getLocation().getY(),pBox->getLocation().getZ()));
		mass = 0.;
		break;
	case PHY_Convex:
		pConvex = (PHYConvex *)ipObj;
		count = wo.loadObj(pConvex->GetFileName());
		trimesh = new btTriangleMesh();
		btVector3 localScaling(0.f,0.f,0.f);
		int i;
		for ( i=0;i<wo.mTriCount;i++)
		{
			int index0 = wo.mIndices[i*3];
			int index1 = wo.mIndices[i*3+1];
			int index2 = wo.mIndices[i*3+2];

			btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
			btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
			btVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
			
			vertex0 *= localScaling;
			vertex1 *= localScaling;
			vertex2 *= localScaling;

			trimesh->addTriangle(vertex0,vertex1,vertex2);
		}
		tmpConvexShape = new btConvexTriangleMeshShape(trimesh);
		hull = new btShapeHull(tmpConvexShape);
		btScalar margin = tmpConvexShape->getMargin();
		hull->buildHull(margin);
		tmpConvexShape->setUserPointer(hull);
		convexShape = new btConvexHullShape();
		for (i=0;i<hull->numVertices();i++)
		{
			convexShape->addPoint(hull->getVertexPointer()[i]);	
		}
		delete tmpConvexShape;
		delete hull;

		shape = convexShape;
		location.setOrigin(btVector3(pConvex->getLocation().getX(),pConvex->getLocation().getY(),pConvex->getLocation().getZ()));
		mass = 10.;
		break;
	}
	
	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		shape->calculateLocalInertia(mass,localInertia);
	btScalar restitution(0.8);

	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(location);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
	body->setRestitution(restitution);
	if(!isDynamic)
		body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
	//btCollisionObject *pColObj = new btCollisionObject();

	//add the body to the dynamics world
	_dynamicsWorld->addRigidBody(body);

	_CollisionBodies.push_back(body);

}
int PHYModeler::GetNbCollisionObjects()
{
	return _CollisionObjects.size();
}
PHYCollisionObj* PHYModeler::GetCollisionObject(int iIndex)
{
	list<PHYCollisionObj*>::iterator collisionIter;
	PHYCollisionObj *pObj = NULL;
	for(collisionIter = _CollisionObjects.begin(); collisionIter != _CollisionObjects.end(); collisionIter++)
	{
		pObj = (*collisionIter);
		if(pObj && (iIndex == pObj->getIndex()))
			return pObj;
		else
			pObj = NULL;
	}
	return pObj;
}

void PHYModeler::_CreateGround()
{

	///create a few basic rigid bodies
	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(1)));

	//keep track of the shapes, we release memory at exit.
	//make sure to re-use collision shapes among rigid bodies whenever possible!
	//btAlignedObjectArray<btCollisionShape*> collisionShapes;

	//collisionShapes.push_back(groundShape);

	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,-35,0));

	{
		btScalar mass(0.);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			groundShape->calculateLocalInertia(mass,localInertia);

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		//add the body to the dynamics world
		_dynamicsWorld->addRigidBody(body);
	}
}

void PHYModeler::UpdateLocations()
{
	btTransform location;
	btMotionState* myMotionState = NULL;
	int nbObj = _dynamicsWorld->getNumCollisionObjects();
	if(0 == nbObj)
		return;

	btCollisionObject *pColObj = _dynamicsWorld->getCollisionObjectArray()[1];

	btRigidBody* body = NULL;
	PHYCollisionObj *pObj = NULL;
	for(int nIte = 0; nIte < nbObj; nIte++)
	{
		body = _CollisionBodies[nIte];		
		pObj = GetCollisionObject(nIte+1);
		if(body && pObj)
		{
			myMotionState = body->getMotionState();
			if(myMotionState)
				myMotionState->getWorldTransform(location);
			else continue;
			//PHYVector vectprev(pObj->getLocation());
			PHYVector vect(location.getOrigin().getX(),location.getOrigin().getY(),location.getOrigin().getZ());
			pObj->setLocation(vect);
		}
		else continue;
	}
}

